#include "particle.h"

#include <gl\glfw.h>

void SpringBody::draw()
{

	//for(size_t i=0; i<particles.size(); i++)
	//{
	//	const Vec2f &pos = particles[i]->pos;
	//	glBegin(GL_POINTS);
	//		glVertex2f( (float)pos.x(), (float)pos.y() );
	//	glEnd();
	//}

	if( isColliding )
	{
		glColor3f(1,0,0);
	}else{
		glColor3f(1,1,1);
	}

	for(size_t i=0; i<edges.size(); i++)
	{
		Edge *e = edges[i];
		//if( e->inUse() )
		{
			const Vec2f &pos1 = e->particles[0]->pos;
			const Vec2f &pos2 = e->particles[1]->pos;
			glBegin(GL_LINES);
			glVertex3f( (float)pos1.x(), (float)pos1.y(), 0 );
			glVertex3f( (float)pos2.x(), (float)pos2.y(), 0 );

			//glVertex3f( (float)pos1.x(), (float)pos1.y(), -.5f );
			//glVertex3f( (float)pos2.x(), (float)pos2.y(), -.5f );
			glEnd();
		}

	}

	calcCenter();

	glPushMatrix();
	//real rotation = atan2( particles[1]->pos.y() - particles[0]->pos.y(), particles[1]->pos.x() - particles[0]->pos.x() );

	//  + 45.0f
	//glTranslatef( (float)centroid.x(), (float)centroid.y(), 0.0f);
	//glRotatef( rotation / 1.57f * 90.0f, 0, 0, 1 );
	//glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	//glColor3f(1, 1, 0.25);
	//Shapes3D::DrawCube(0.25);
	//glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	glPopMatrix();


}

void SpringBody::calcCenter()
{
	oldCentroid = centroid;
	centroid.is(0.0,0.0);
	bounds.xmin = 1e32f;
	bounds.ymin = 1e32f;
	bounds.xmax = -1e32f;
	bounds.ymax = -1e32f;

	assert( particles.size() > 1 );

	for(size_t i=0; i<particles.size(); i++)
	{
		Particle *p = particles[i];
		centroid += p->pos;
		bounds.xmin = std::min<real>( bounds.xmin, p->pos.x() );
		bounds.ymin = std::min<real>( bounds.ymin, p->pos.y() );
		bounds.xmax = std::max<real>( bounds.xmax, p->pos.x() );
		bounds.ymax = std::max<real>( bounds.ymax, p->pos.y() );
	}
	centroid /= (real)particles.size();

	obb.computeAxes();
}

void SpringBody::addForce( Vec2f &force )
{
	for(size_t i=0; i<particles.size(); i++){
		Particle *p = particles[i];
		p->forceAccum += force;
	}
}

void SpringBody::setFrozen()
{
	isFrozen = true;
	for each(Particle *p in particles)
	{
		p->massInverse = 0.0;
	}
}

// Todo find out why it doesnt work as expected with World::isSpaceEmpty( xmin, ymin, xmax, ymax )
bool SpringBody::bodiesOverlap( SpringBody *other )
{
	return this->bounds.intersects( other->getBounds() );
}



SpringBody::SpringBody()
{
	isFrozen = false;
	isColliding = false;
	isAlive = false;
	bounds.parent = this;
	name = "un-initialized";
}

void SpringBody::calcBoundryEdges()
{
	for each(Edge *currentEdge in edges)
	{
		currentEdge->isBoundary = true;
		Vec2f edgeNormal(currentEdge->particles[0]->pos.y() - currentEdge->particles[1]->pos.y(), currentEdge->particles[1]->pos.x() -currentEdge->particles[0]->pos.x() );
		int firstSign = SGN( edgeNormal.dot( particles[0]->pos - currentEdge->particles[0]->pos) );

		for each(Particle *p in particles)
		{
			if( p == currentEdge->particles[0] ) continue;
			if( p == currentEdge->particles[1] ) continue;
			if( p == particles[0] ) continue;

			//Vec2f edgeNormal(e->particles[0]->pos.y() - e->particles[1]->pos.y(), e->particles[1]->pos.x() - e->particles[0]->pos.x() );
			int sign = SGN( edgeNormal.dot( p->pos - currentEdge->particles[0]->pos) );

			if( sign != firstSign )
			{
				currentEdge->isBoundary = false;
				break;
			}
		}
	}

}

void SpringBody::setUnused()
{
	// Todo: replace with fixed size arrays.
	// with consts max body verts, max body edges
	particles.clear();
	edges.clear();
	
}

BBox* SpringBody::getBounds()
{
	return &bounds;
}

OBB2D * SpringBody::getOBB()
{
	return &obb;
}



Edge::Edge( Particle *p1, Particle *p2 )
{
	init(p1,p2);
}

Edge::Edge()
{
	particles[0] = 0x0;
	particles[1] = 0x0;
	parent = 0x0;
}

void Edge::init( Particle *p1, Particle *p2 )
{
	this->isBoundary = true;
	particles[0] = p1;
	particles[1] = p2;
	parent = 0x0;
	originalLength = length( p1->pos - p2->pos );
}



void BBox::draw()
{
	glBegin(GL_LINE_LOOP);
	glVertex2f(xmin,ymin); 
	glVertex2f(xmax,ymin); 
	glVertex2f(xmax,ymax); 
	glVertex2f(xmin,ymax); 
	glEnd();
}
